#include "icm42607.h"
#include "iic1.h"

#define ICM42607_DEVICE_ADDR 0x68
#define WHO_AM_I 0x75
								
// 判断芯片是否存在
static uint8_t existenceFlag = 0;	
									
uint8_t icm42607Init(void) 			
{									
	uint8_t id = 0;
	i2c1Read(ICM42607_DEVICE_ADDR,WHO_AM_I, &id, 1);

	if(id == 0x61)
	{
		existenceFlag = 1;
								
		uint8_t regData = 0x06;	
		i2c1Write(ICM42607_DEVICE_ADDR, 0x20, &regData, 1);//配置GURO输出频率和量程//800hz
		i2c1Write(ICM42607_DEVICE_ADDR, 0x21, &regData, 1);//配置ACC输出频率和量程
						
		regData=0x33;	
		i2c1Write(ICM42607_DEVICE_ADDR,0x23, &regData, 1);//gyro bandwidth 73hz
		regData=0x43;	
		i2c1Write(ICM42607_DEVICE_ADDR,0x24, &regData, 1);//ACC bandwidth 32*average 73hz
						
		regData = 0x0f;
		i2c1Write(ICM42607_DEVICE_ADDR, 0x1f, &regData, 1);//主要是开启陀螺仪，并配置功耗
		
	}
	return existenceFlag;
}

// 先用int16来计算校准值，再用float来算出最后结果
// 防止用float来校准丢失精度 
void getIcm42607Data(int16_t *acc, int16_t *gyro)
{
	if (existenceFlag)
	{	
		uint8_t data[12];
		i2c1Read(ICM42607_DEVICE_ADDR,0x0b, data, 12);
		int16_t t_sensorAcc[3];
		int16_t t_sensorGyro[3];
		t_sensorAcc[X] = (int16_t)(data[1] | (data[0] << 8));
		t_sensorAcc[Y] = (int16_t)(data[3] | (data[2] << 8));
		t_sensorAcc[Z] = (int16_t)(data[5] | (data[4] << 8));
		t_sensorGyro[X] = (int16_t)(data[7] | (data[6] << 8));
		t_sensorGyro[Y] = (int16_t)(data[9] | (data[8] << 8));
		t_sensorGyro[Z] = (int16_t)(data[11] | (data[10] << 8));
			
		acc[Y] = t_sensorAcc[X];
		acc[X] = t_sensorAcc[Y];
		acc[Z] = t_sensorAcc[Z];
		gyro[Y] = -t_sensorGyro[X];
		gyro[X] = -t_sensorGyro[Y];
		gyro[Z] = t_sensorGyro[Z];
	}
}


